package ar.fi.uba.celdas.ambiente.gui;


import ar.fi.uba.celdas.ambiente.modelo.IRobot;

public class RobotInterpolado{

	private int pasos;
	private final IRobot robot;
	private int paso = 0;
	private final FloatPoint posicionAnterior = new FloatPoint();
	private final FloatPoint direccionAnterior = new FloatPoint();
	private final FloatPoint posicionInterpolada = new FloatPoint();
	private final FloatPoint direccionInterpolada = new FloatPoint();
	
	public RobotInterpolado(IRobot robot, int pasos) {
		this.pasos = pasos;
		this.robot = robot;
		posicionAnterior.setLocation(robot.getPosicion());
		direccionAnterior.setLocation(robot.getVectorDireccion());
	}
	
	public void mover() {
		if (++paso > pasos ){
			paso = 0;
			posicionAnterior.setLocation(robot.getPosicion());
			direccionAnterior.setLocation(robot.getVectorDireccion());
			robot.mover();
		}
		
		double progreso = paso / (double)pasos;
		interpolarPosicion(progreso);
		
		interpolarDireccion(progreso);		
	}

	private void interpolarDireccion(double progreso) {
		//giro de 180 grados en X
		if (direccionAnterior.x!= 0  && 
				direccionAnterior.x == - robot.getVectorDireccion().x ){
			if (paso < pasos / 2){
				progreso = 2 * progreso;
				direccionInterpolada.x = direccionAnterior.x * (1 -progreso );
				direccionInterpolada.y = progreso;
			} else {
				progreso = 2 * (progreso - .5f);
				direccionInterpolada.x = robot.getVectorDireccion().x * progreso;
				direccionInterpolada.y = 1 - progreso;
			}
		}
		//giro de 180 grados en Y
		else if (direccionAnterior.y!= 0  && 
				direccionAnterior.y == - robot.getVectorDireccion().y ){
			if (paso < pasos / 2){
				progreso = 2 * progreso;
				direccionInterpolada.y = direccionAnterior.y * (1 -progreso );
				direccionInterpolada.x = progreso;
			} else {
				progreso = 2 * (progreso - .5f);
				direccionInterpolada.y = robot.getVectorDireccion().y * progreso;
				direccionInterpolada.x = 1 - progreso;
			}
		}	
		else{
			//caso ideal, solo 90 grados
			direccionInterpolada.x = 
					direccionAnterior.x * (1 -progreso)
					+ robot.getVectorDireccion().x * progreso;
			direccionInterpolada.y = 
					direccionAnterior.y * (1 -progreso)
					+ robot.getVectorDireccion().y * progreso;
		}
	}

	private void interpolarPosicion(double progreso) {
		posicionInterpolada.x = 
				posicionAnterior.x * (1 -progreso)
				+ robot.getPosicion().x * progreso;
		posicionInterpolada.y = 
				posicionAnterior.y * (1 -progreso)
				+ robot.getPosicion().y * progreso;
	}

	public void setPasos(int pasos) {
		this.pasos = pasos;
	}
	
	public int getPasos() {
		return pasos;
	}
	
	public FloatPoint getPosicion() {
		return posicionInterpolada;
	}

	public FloatPoint getVectorDireccion() {
		return direccionInterpolada;
	}
	
	public double getRotacion() {
		double x = direccionInterpolada.x;
		double y = direccionInterpolada.y;
		if (x < 0)
			return Math.acos(y / Math.sqrt(x*x + y*y));
		else
			return -Math.acos(y / Math.sqrt(x*x + y*y));
	}

}
